#include "alg3d/Mat4.h"
#include "alg3d/AxisAngle.h"
#include <iostream>

namespace gx
{

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat4 operator*(const Mat4 & a, const Mat4 & b)
{
    Mat4 result; 
    result.rotation = a.rotation * b.rotation;
    result.translation = a.rotation * b.translation + a.translation;
    return result;
}

Mat4 operator*(Mat4 const & m4, Mat3 const & m3) 
{
    return m4 * Mat4().make_rot(m3);
}

Mat4 operator*(Mat3 const & m3, Mat4 const & m4) 
{
    return Mat4().make_rot(m3) * m4;
}

Mat4 & Mat4::concat(const Mat4 & second)
{
    *this = second * *this;
    return *this;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat4::Real4 * Mat4::to_2d_array(Real4 m[4]) const
{
    rotation.col_0.to_array(m[0]);
    rotation.col_1.to_array(m[1]);
    rotation.col_2.to_array(m[2]);
    translation.to_array(m[3]);
    m[0][3] = m[1][3] = m[2][3] = 0.0;
    m[3][3] = 1.0;
    return m;
}

Real * Mat4::to_array(Real m[16]) const
{	
    rotation.col_0.to_array(m + 0);
    rotation.col_1.to_array(m + 4);
    rotation.col_2.to_array(m + 8);
    translation.to_array(m + 12);
    m[3] = m[7] = m[11] = 0.0;
    m[15] = 1.0;
    return m;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat4 & Mat4::invert_affine()
{
    rotation.transpose();
    translation = -(rotation * translation);
    return *this;
}

Mat4 & Mat4::invert()
{
    double m00 = rotation.col_0.x, m01 = rotation.col_0.y, m02 = rotation.col_0.z, m03 = 0.;
    double m10 = rotation.col_1.x, m11 = rotation.col_1.y, m12 = rotation.col_1.z, m13 = 0.;
    double m20 = rotation.col_2.x, m21 = rotation.col_2.y, m22 = rotation.col_2.z, m23 = 0.;
    double m30 = translation.x, m31 = translation.y, m32 = translation.z, m33 = 1.;

    double v0 = m20 * m31 - m21 * m30;
    double v1 = m20 * m32 - m22 * m30;
    double v2 = m20 * m33 - m23 * m30;
    double v3 = m21 * m32 - m22 * m31;
    double v4 = m21 * m33 - m23 * m31;
    double v5 = m22 * m33 - m23 * m32;

    double t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
    double t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
    double t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
    double t30 = - (v3 * m10 - v1 * m11 + v0 * m12);

    double invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);

    double d00 = t00 * invDet;
    double d10 = t10 * invDet;
    double d20 = t20 * invDet;
    double d30 = t30 * invDet;

    double d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
    double d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
    double d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
    double d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

    v0 = m10 * m31 - m11 * m30;
    v1 = m10 * m32 - m12 * m30;
    v2 = m10 * m33 - m13 * m30;
    v3 = m11 * m32 - m12 * m31;
    v4 = m11 * m33 - m13 * m31;
    v5 = m12 * m33 - m13 * m32;

    double d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
    double d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
    double d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
    double d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

    v0 = m21 * m10 - m20 * m11;
    v1 = m22 * m10 - m20 * m12;
    v2 = m23 * m10 - m20 * m13;
    v3 = m22 * m11 - m21 * m12;
    v4 = m23 * m11 - m21 * m13;
    v5 = m23 * m12 - m22 * m13;

    double d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
    double d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
    double d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
    double d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

    *this = gx::Mat4(
        d00, d10, d20, d30,
        d01, d11, d21, d31,
        d02, d12, d22, d32);
    return *this;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

std::ostream & operator<<(std::ostream &os, const gx::Mat4 &val)
{
    gx::Vec3 col0 = val.get_col_0(),
             col1 = val.get_col_1(),
             col2 = val.get_col_2(),
             col3 = val.get_col_3();
    os << zero_if_tiny(col0.x) << " " << zero_if_tiny(col1.x) << " " 
       << zero_if_tiny(col2.x) << " " << zero_if_tiny(col3.x) << "\n";
    os << zero_if_tiny(col0.y) << " " << zero_if_tiny(col1.y) << " " 
       << zero_if_tiny(col2.y) << " " << zero_if_tiny(col3.y) << "\n";
    os << zero_if_tiny(col0.z) << " " << zero_if_tiny(col1.z) << " " 
       << zero_if_tiny(col2.z) << " " << zero_if_tiny(col3.z) << "\n";
    os << "0 0 0 1";
    return os;
}

std::istream & operator>>(std::istream &is, Mat4 &val)
{
    Real r00, r01, r02, r03, r10, r11, r12, r13, r20, r21, r22, r23, dummy;
    is >>	r00 >> r01 >> r02 >> r03 >> 
            r10 >> r11 >> r12 >> r13 >> 
            r20 >> r21 >> r22 >> r23 >> 
            dummy >> dummy >> dummy >> dummy;
    val = gx::Mat4(	r00, r01, r02, r03,
                    r10, r11, r12, r13,
                    r20, r21, r22, r23);
    return is;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat4 & Mat4::make_scale(double s)
{
    translation = gx::zero;
    rotation.make_scale(s);
    return *this;
}

Mat4 & Mat4::make_scale(const Vec3 & s)
{
    translation = gx::zero;
    rotation.make_scale(s);
    return *this;
}

Mat4 & Mat4::make_pivot_scale(const Vec3 & pivot, double s)
{
    *this = gx::Mat4().make_transl(pivot) * gx::Mat3().make_scale(s) 
             * gx::Mat4().make_transl(-pivot);
    return *this;
}

Mat4 & Mat4::make_pivot_scale(const Vec3 & pivot, const Vec3 & s)
{
    *this = gx::Mat4().make_transl(pivot) * gx::Mat3().make_scale(s) 
             * gx::Mat4().make_transl(-pivot);
    return *this;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


Mat4 & Mat4::make_transl(double tx, double ty, double tz)
{
    rotation = gx::eye;
    translation = gx::Vec3(tx, ty, tz);
    return *this;
}

Mat4 & Mat4::make_transl(const Vec3 & translation)
{
    return make_transl(translation.x, translation.y, translation.z);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


Mat4 & Mat4::make_rot(const gx::Mat3 & rotation)
{
    this->rotation = rotation;
    translation = gx::zero;
    return *this;
}

Mat4 & Mat4::make_rot(const gx::AxisAngle & aa)
{
    return make_rot(Mat3().make_rot(aa));
}

Mat4 & Mat4::make_pivot_rot(const Vec3 & pivot, const Mat3 & rot)
{
    *this = Mat4().make_transl(pivot) 
          * Mat4().make_rot(rot) 
          * Mat4().make_transl(-pivot);
    return *this;
}

} // namespace gx


